#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "string.h"
#include "freertos/ringbuf.h"
#include "esp_timer.h"

#include "cmd/cmd.h"
#include "MotorCtrl.h"
#include "Rs485Interface/Rs485Interface.h"
#include "driver/uart.h"
#include "PosSensor/PosSensor.h"

#define MaxNumController        6               //单个控制器做多控制轴的数量
#define MinMoveDistance         1               //单次最小移动距离 mm
#define PosDir                  0               //0:向下为正方向  1：向上为正方向

extern CmdHandle_t CmdHandle;
extern QueueHandle_t MotorCodeQueue;
static const char *Tag = "MotorCtrl";
TimerHandle_t UpdataMotorStateTimer;
char MotorSendBuf[30];
extern bool WaitReply;

void UpdataMotorState(TimerHandle_t xTimer);
static void SendInsToAxis(MotorAxis_t Axis, const void *data, size_t size);

void MotorCtrl_task(void *pvParameters) 
{
    MotorPosState_t MotorPosState;
    UpdataMotorStateTimer  = xTimerCreate("UpdataMotorStateTimer", pdMS_TO_TICKS(500), true, NULL, UpdataMotorState);
    xTimerStart(UpdataMotorStateTimer, portMAX_DELAY);

    for (;;) 
    {
        if(xQueueReceive(MotorCodeQueue, &MotorPosState, portMAX_DELAY) == pdPASS)
        {
            switch (MotorPosState.MotorRunState)
            {
                case MotorIdle:
                    ESP_LOGI(Tag, "motor idle");
                    break;
                case MotorUpMove:
                    // if((MotorPosState.MotorState == MotorStop) && (WaitReply == 0))
                    if(WaitReply == 0)
                    {
                        if(getZeroPos(MotorPosState.MotorAxis) - getMotorPos(MotorPosState.MotorAxis) < TrayTravel)  
                        {
                            uint16_t len = gcodeMoveFast(MotorPosState.MotorAxis, PosDir ? MinMoveDistance : (-MinMoveDistance), MotorSendBuf);
                            SendInsToAxis(MotorPosState.MotorAxis, MotorSendBuf, len);
                        }
                    }
                    break;
                case MotorDowmMove:
                    // if((MotorPosState.MotorState == MotorStop) && (WaitReply == 0))
                    if(WaitReply == 0)
                    {
                        uint16_t len = gcodeMoveFast(MotorPosState.MotorAxis, PosDir ? (-MinMoveDistance) : MinMoveDistance, MotorSendBuf);
                        SendInsToAxis(MotorPosState.MotorAxis, MotorSendBuf, len);
                    }
                    break;
                default:
                    ESP_LOGI(Tag, "Motor Run State error");
                    break;
            }
        }
    }
    vTaskDelete(NULL);
}

void UpdataMotorState(TimerHandle_t xTimer)
{
    char sendbuf[5] = {0};
    uint16_t len = gcodeQuerisePos(sendbuf);
    Rs485SendData(RS485_1DEPIN, RS4851_UARTNUM, sendbuf, len);
}

static void SendInsToAxis(MotorAxis_t Axis, const void *data, size_t size)
{
    if(Axis < MaxNumController)
    {
        Rs485SendData(RS485_1DEPIN, RS4851_UARTNUM, data, size);
    }
    else
    {
        Rs485SendData(RS485_2DEPIN, RS4852_UARTNUM, data, size);
    }
}

